/*
dryer_door.c
offer dryer door control function realize
designed by lunanting

----------------------
2020-11-10 11:18:45
create file
----------------------
*/
#include "dryer_door.h"
#include "../dm556_driver.h"
#include "k_api.h"


static DRYER_DOOR door={0};

static int xp_dryerDoor_stop(DRYER_DOOR_door *dr);
static void xp_dryerDoor_left_thread(void *arg);
static void xp_dryerDoor_right_thread(void *arg);

/*
dyer door init
param:
    nodeL:left door node
    nodeR:right door node
    speed:motor speed r/min
return:
    0=success
*/
int xp_dryerDoor_init(u8 nodeL,u8 nodeR,u16 speed){
    int sta=0;
    s_profile_position_config config;
    can_node_id_t can;

    if(xp_canopen_dev_manager_init()==-1){
        perr("can open init fail~");
        return -1;
    }

    door.left_door.port=1;
    door.left_door.node=nodeL;
    door.right_door.port=1;
    door.right_door.node=nodeR;
    door.speed=speed;
    door.left_door.mark=0;
    door.right_door.mark=1;
    aos_msleep(2000);
    can.port=1;
    //left door
    can.node=door.left_door.node;
    sta=enable_dm556(&can);
    if(sta!=0){
        perr("left door driver enable fail~");
        return -2;
    }
    config.motor_resolution=2000;
    door.right_door.pluse_speed=door.left_door.pluse_speed=speed/60*2000;
    config.profile_velocity=door.left_door.pluse_speed;
    config.end_velocity=2000;
    config.profile_acceleration=20000;
    config.profile_deceleration=20000;
    sta=profile_position_config(&can,&config);
    if(sta!=0){
        perr("config left door motor fail~");
        return -3;
    }
    door.left_door.location=0;
    //right door
    can.node=door.right_door.node;
    sta=enable_dm556(&can);
    if(sta!=0){
        perr("right door driver enable fail~");
        return -2;
    }
    sta=profile_position_config(&can,&config);
    if(sta!=0){
        perr("config right door motor fail~");
        return -3;
    }
    door.right_door.location=0;

    
    if(aos_task_new("left door",xp_dryerDoor_left_thread,NULL,2100)!=0){
        perr("left door thread create fail ~");
        return -4;
    }
    if(aos_task_new("right door",xp_dryerDoor_right_thread,NULL,2100)!=0){
        perr("right door thread create fail ~");
        return -5;
    }
    aos_mutex_new(&door.left_door.mux);
    aos_mutex_new(&door.right_door.mux);
    door.init=1;
    println("dryer door init success~");
    return 0;
}


/*
door move
param:
    dis: move distance :mm
return:
    0=success
*/
int xp_dryerDoor_move(DRYER_DOOR_door *dr,int dis){
    int sta=0;
    int pluse;
    int cha;
    int delay;
    u64 time_start;
    can_node_id_t can;

    if(dr->error==1)return -1;
    if(aos_mutex_lock(&dr->mux,10000)!=0){
        perr("mux wait timeout~ door node=%d",dr->node);
        return 1;
    }
    can.port=1;
    can.node=dr->node;
    if(dis==dr->location){
        dr->dir=0;
        aos_mutex_unlock(&dr->mux);
        return 0;
    }
    else if(dis>dr->location){
        dr->dir=1;
        if(dr->mark==0){    //左门
            if(xp_io_read_input_pin(1,DRYER_DOOR_limit_left_close)==DRYER_DOOR_io_touched){
                dr->location=130;
                aos_mutex_unlock(&dr->mux);
                return 0;
            }
        }
        else if(dr->mark==1){   //右门
            if(xp_io_read_input_pin(1,DRYER_DOOR_limit_right_close)==DRYER_DOOR_io_touched){
                dr->location=130;
                aos_mutex_unlock(&dr->mux);
                return 0;
            }
        }
        else{
            perr("no matched mark~~");
            dr->error=1;
            aos_mutex_unlock(&dr->mux);
            return -1;
        }
    }
    else if(dis<dr->location){
        dr->dir=-1;
        if(dr->mark==0){
            if(xp_io_read_input_pin(1,DRYER_DOOR_limit_left_open)==DRYER_DOOR_io_touched){
                dr->location=0;
                aos_mutex_unlock(&dr->mux);
                return 0;
            }
        }
        else if(dr->mark==1){
            if(xp_io_read_input_pin(1,DRYER_DOOR_limit_right_open)==DRYER_DOOR_io_touched){
                dr->location=0;
                aos_mutex_unlock(&dr->mux);
                return 0;
            }
        }
        else{
            perr("no matched mark~~");
            dr->error=1;
            aos_mutex_unlock(&dr->mux);
            return -1;
        }
    }

    cha=dis-dr->location;
    pluse=cha*2000*DRYER_DOOR_ratio/DRYER_DOOR_dis;
    println("door motor will move to %d position,over %d mm,%d pluse ~",dis,cha,pluse);
    sta=relative_position_move(&can,pluse);
    time_start=aos_now_ms();
    dr->location=dis;
    delay=((float)abs(pluse)/dr->pluse_speed+((float)dr->pluse_speed/18000))*1000;
    while(1){
        if((aos_now_ms()-time_start)>delay){
            println("time out~");
            break;
        }
        if(dr->dir==-1){
            if(dr->mark==0){
                if(xp_io_read_input_pin(1,DRYER_DOOR_limit_left_open)==DRYER_DOOR_io_touched){
                    dr->location=0;
                    xp_dryerDoor_stop(dr);
                    println("left door open limit io touched !!");
                    break;
                }
            }
            else if(dr->mark==1){
                if(xp_io_read_input_pin(1,DRYER_DOOR_limit_right_open)==DRYER_DOOR_io_touched){
                    dr->location=0;
                    xp_dryerDoor_stop(dr);
                    println("right door open limit io touched !!");
                    break;
                }
            }
        }
        if(dr->dir==1){
            if(dr->mark==0){
                if(xp_io_read_input_pin(1,DRYER_DOOR_limit_left_close)==DRYER_DOOR_io_touched){
                    dr->location=135;
                    xp_dryerDoor_stop(dr);
                    println("left door close limit io touched !!");
                    break;
                }
            }
            else if(dr->mark==1){
                if(xp_io_read_input_pin(1,DRYER_DOOR_limit_right_close)==DRYER_DOOR_io_touched){
                    dr->location=135;
                    xp_dryerDoor_stop(dr);
                    println("right door close limit io touched !!");
                    break;
                }
            }
        }
        aos_msleep(50);
    }
    dr->dir=0;
    aos_mutex_unlock(&dr->mux);
    return 0;
}



/*
dryer door stop
return:
    0=success
*/
static int xp_dryerDoor_stop(DRYER_DOOR_door *dr){
    can_node_id_t can;

    can.port=1;
    can.node=dr->node;
    dr->dir=0;
    println("dryer door stop ~");
    return slow_down(&can,50000);
}


/*
move door set
param:
    dr:door obj
    dis: set move distance
return:
    0=success
*/
int xp_dryerDoor_move_set(DRYER_DOOR_door *dr,int dis){
    if(door.init!=1)return -1;
    if(door.left_door.error>0)return -2;
    if(door.right_door.error>0)return -3;

    dr->target=dis;
    aos_sem_signal(&dr->sem);
    return 0;
}


/*
clear door move
param:
    dr:door obj
*/
void xp_dryerDoor_move_clear(DRYER_DOOR_door *dr){
    dr->target=dr->location;
    krhino_sem_count_set(dr->sem.hdl,0);
    xp_dryerDoor_stop(dr);
}

/*
dryer door all set
param:
    dis: set move distance
return:
    0=success
*/
int xp_dryerDoor_all_set(int dis){
    if(door.init!=1)return -1;
    if(door.left_door.error>0)return -2;
    if(door.right_door.error>0)return -3;

    door.targe_location=dis;
    door.left_door.target=dis;
    door.right_door.target=dis;
    aos_sem_signal(&door.left_door.sem);
    aos_sem_signal(&door.right_door.sem);
    return 0;
}

/*
dryer door all move clear
*/
void xp_dryerDoor_all_clear(void){
    door.left_door.target=door.left_door.location;
    door.right_door.target=door.right_door.location;
    krhino_sem_count_set(door.left_door.sem.hdl,0);
    krhino_sem_count_set(door.right_door.sem.hdl,0);
    xp_dryerDoor_stop(&door.left_door);
    xp_dryerDoor_stop(&door.right_door);
}


/*
dryer door set
param:
    sta:set state,0=close,1=open
return:
    0=success
*/
int xp_dryerDoor_set(u8 sta){
    if(sta==0){
        return xp_dryerDoor_all_set(135);
    }
    else{
        return xp_dryerDoor_all_set(-135);
    }
}

/*
left door thread
*/
static void xp_dryerDoor_left_thread(void *arg){
    aos_sem_t *sem=&door.left_door.sem;
    aos_sem_new(sem,0);

    while(1){
        aos_sem_wait(sem,AOS_WAIT_FOREVER);
        xp_dryerDoor_move(&door.left_door,door.left_door.target);

    }
}

/*
left door thread
*/
static void xp_dryerDoor_right_thread(void *arg){
    aos_sem_t *sem=&door.right_door.sem;
    aos_sem_new(sem,0);

    while(1){
        aos_sem_wait(sem,AOS_WAIT_FOREVER);
        xp_dryerDoor_move(&door.right_door,door.right_door.target);
        
    }
}



/**************************debug***************************/
/*
dryer door debug
*/
int xp_dryerDoor_debug(char *type,char *fun,char *param){
    int i;
    if(strcmp(type,"dryerDoor")!=0)return 0;
    if(strcmp(fun,"left_door")==0){
        i=atoi(param);
        xp_dryerDoor_move_set(&door.left_door,i);
    }
    else if(strcmp(fun,"right_door")==0){
        i=atoi(param);
        xp_dryerDoor_move_set(&door.right_door,i);
    }
    else if(strcmp(fun,"open")==0){
        xp_dryerDoor_set(1);
    }
    else if(strcmp(fun,"close")==0){
        xp_dryerDoor_set(0);
    }
    else if(strcmp(fun,"move")==0){
        i=atoi(param);
        xp_dryerDoor_all_set(i);
    }
    else{
        return 0;
    }
    return 1;
}



//end of the file
